from __future__ import print_function

from threading import Thread

import can
import time
from enum import Enum
import keyboard

accSlow = 20
accFast = 40
accBack = 15
brakeSpace = 50
brakeNormal = 30
brakeChange = 30




class CarState(Enum):
    zero_forward = 0
    zero_backward = 1
    zero_neutral = 2

    backward = 3
    neutral_backward = 4

    forward = 5
    fast_forward = 6
    neutral_forward = 7


class CarController():
    steer_exp = 0
    vcu_speed = 0
    vcu_acc = 0
    eps_steer = 0
    vcu_gear = 1
    current_brake = 0       # 刹车无法在can信息中获得，在move中改变
    accSlow = 20
    accFast = 40
    accBack = 15
    steerDelta = 20
    brakeSpace = 40
    brake = 20

    state_table = {
        (CarState.zero_forward, 'w'): lambda self, steer: CarController.move(self,accSlow, 0, steer, 1),
        (CarState.zero_forward, 's'): lambda self, steer: CarController.move(self,0, brakeNormal, steer, 1),
        (CarState.zero_forward, 'space'): lambda self, steer: CarController.move(self,0, brakeSpace, steer, 1),
        (CarState.zero_forward, 'r'): lambda self, steer: CarController.move(self,0, brakeChange, steer, 0),
        (CarState.zero_forward, 'shift+w'): lambda self, steer: CarController.move(self,accFast, 0, steer, 1),
        (CarState.zero_forward, 'release'): lambda self, steer: CarController.move(self,0, 0, steer, 1),

        (CarState.zero_backward, 'e'): lambda self, steer: CarController.move(self,0, brakeChange, steer, 0),
        (CarState.zero_backward, 's'): lambda self, steer: CarController.move(self,accBack, 0, steer, 2),
        (CarState.zero_backward, 'w'): lambda self, steer: CarController.move(self,0, brakeNormal, steer, 2),
        (CarState.zero_backward, 'space'): lambda self, steer: CarController.move(self,0, brakeSpace, steer, 2),
        (CarState.zero_backward, 'release'): lambda self, steer: CarController.move(self,0, 0, steer, 2),

        (CarState.zero_neutral, 'e'): lambda self, steer: CarController.move(self,0, brakeChange, steer, 1),
        (CarState.zero_neutral, 'r'): lambda self, steer: CarController.move(self,0, brakeChange, steer, 2),

        (CarState.neutral_forward, 'w'): lambda self, steer: CarController.move(self,accSlow, 0, steer, 1),
        (CarState.neutral_forward, 'shift+w'): lambda self, steer: CarController.move(self,accFast, 0, steer, 1),
        (CarState.neutral_forward, 's'): lambda self, steer: CarController.move(self,0, brakeNormal, steer, 1),
        (CarState.neutral_forward, 'space'): lambda self, steer: CarController.move(self,0, brakeSpace, steer, 1),
        (CarState.neutral_forward, 'release'): lambda self, steer: CarController.move(self,0, 0, steer, 1),

        (CarState.neutral_backward, 's'): lambda self, steer: CarController.move(self,accBack, 0, steer, 2),
        (CarState.neutral_backward, 'release'): lambda self, steer: CarController.move(self,0, 0, steer, 2),
        (CarState.neutral_backward, 'w'): lambda self, steer: CarController.move(self,0, brakeNormal, steer, 2),
        (CarState.neutral_backward, 'space'): lambda self, steer: CarController.move(self,0, brakeSpace, steer, 2),

        (CarState.forward, 'w'): lambda self, steer: CarController.move(self,accSlow, 0, steer, 1),
        (CarState.forward, 'shift+w'): lambda self, steer: CarController.move(self,accFast, 0, steer, 1),
        (CarState.forward, 's'): lambda self, steer: CarController.move(self,0, brakeNormal, steer, 1),
        (CarState.forward, 'space'): lambda self, steer: CarController.move(self,0, brakeSpace, steer, 1),
        (CarState.forward, 'release'): lambda self, steer: CarController.move(self,0, 0, steer, 1),

        (CarState.fast_forward, 'w'): lambda self, steer: CarController.move(self,accSlow, 0, steer, 1),
        (CarState.fast_forward, 'shift+w'): lambda self, steer: CarController.move(self,accFast, 0, steer, 1),
        (CarState.fast_forward, 's'): lambda self, steer: CarController.move(self,0, brakeNormal, steer, 1),
        (CarState.fast_forward, 'space'): lambda self, steer: CarController.move(self,0, brakeSpace, steer, 1),
        (CarState.fast_forward, 'release'): lambda self, steer: CarController.move(self,0, 0, steer, 1),

        (CarState.backward, 's'): lambda self, steer: CarController.move(self,accBack, 0, steer, 1),
        (CarState.backward, 'w'): lambda self, steer: CarController.move(self,0, brakeNormal, steer, 1),
        (CarState.backward, 'space'): lambda self, steer: CarController.move(self,0, brakeSpace, steer, 1),
        (CarState.backward, 'release'): lambda self, steer: CarController.move(self,0, 0, steer, 1),
    }


    def __init__(self):
        # self.bus0 = can.interface.Bus(bustype='socketcan', channel='can0', bitrate=500000)
        self.bus0 = can.interface.Bus(bustype='canalystii', channel=0, bitrate=500000)
        self.steerRange = [-550, 550]
        self.selfDriving_req = 1
        self.stopCar_req = 0



    def move(self, acc: int, brake: int, steer: float, gearMod: int) -> None:
        can_data = self.gen_candata(acc, brake, steer, gearMod)
        self.cansend_one(can_data)
        self.current_brake = brake      # 改变刹车状态
        # print(f'current stade: acc={acc}, brake={brake}, steer={steer}, gearMod={gearMod}')

    def gen_candata(self, acc: int, brake: int, steer: float, gearMod: int) -> list:
        if not ((0 <= acc <= 60) and (0 <= brake <= 60) and (-700 <= steer <= 700)):
            raise ValueError
        if not (gearMod == 0 or gearMod == 1 or gearMod == 2):
            raise ValueError
        steer_bus_value = int((steer + 870) * 10)
        steer_low = steer_bus_value & 0x00ff
        steer_high = steer_bus_value >> 8
        candata_5th_byte = (self.stopCar_req << 3) + (gearMod << 1) + self.selfDriving_req

        steeringAngle_speed_req = 0

        candata = [acc, brake, steer_low, steer_high, steeringAngle_speed_req, candata_5th_byte, 0, 0]
        return candata

    def startup(self) -> None:
        print("正在启动")

        self.move(acc=0,brake=brakeChange,steer=0,gearMod=0)
        time.sleep(1)
        self.move(acc=0, brake=brakeChange, steer=0, gearMod=1)

        print("已启动")

    def cansend_one(self, can_data, *, arbitration_id=0x0C01D0E1, is_extended_id=True):
        msg = can.Message(arbitration_id=arbitration_id,
                          data=can_data,
                          is_extended_id=is_extended_id)
        try:
            self.bus0.send(msg)
            # print("Message sent on {}".format(self.bus0.channel_info))
        except can.CanError:
            print("Message NOT sent")

    def recv(self) -> can.Message:
        while True:
            bus = self.bus0
            try:
                msg = bus.recv(timeout=5)  # 接收信息
                self.set_state(msg)
            except can.CanError:
                print('Message receiving timeout')
                continue

    def set_state(self, msg: can.Message) -> None:
        msg_data = msg.data
        if msg.arbitration_id == 0x0165:  # 转角反馈，EPS节点0x165
            eps_steer = (msg_data[1] << 8) + msg_data[2]
            self.eps_steer = eps_steer

        elif msg.arbitration_id == 0x18F101D0:
            vcu_speed = ((msg_data[1] << 8) + msg_data[2]) / 256  # unit: km/h
            vcu_acc = msg_data[6]  # unit: %
            vcu_gear = msg_data[3] & 0x03  # 0：空挡；1：前进；2：倒挡
            self.vcu_acc = vcu_acc
            self.vcu_speed = vcu_speed
            self.vcu_gear = vcu_gear

    def get_state(self):
        acc = self.vcu_acc
        speed = self.vcu_speed
        brake = self.current_brake
        gear = self.vcu_gear

        if gear == 0:
            return CarState.zero_neutral
        elif gear == 1:
            if speed == 0:
                return CarState.zero_forward
            elif speed > 1:
                if 20 <= acc:
                    return CarState.fast_forward
                elif 0 < acc < 20:
                    return CarState.forward
                elif acc == 0 and brake == 0:
                    return CarState.neutral_forward
                elif acc == 0 and brake >= 0:
                    return CarState.zero_forward
                else:
                    raise ValueError
            else:
                raise ValueError
        elif gear == 2:
            if speed <= 1:
                return CarState.zero_backward
            elif speed > 1:
                if 0 < acc:
                    return CarState.backward
                elif acc == 0 and brake == 0:
                    return CarState.neutral_backward
                elif acc == 0 and brake > 0:
                    return CarState.zero_backward
                else:
                    raise ValueError
            else:
                raise ValueError
        else:
            raise ValueError

    def get_key_x(self):    # 纵向控制
        key_list = keyboard.get_hotkey_name().split('+')

        if 'space' in key_list:
            return 'space'
        elif 's' in key_list or 'S' in key_list:
            return 's'
        elif 'w' in key_list or 'W' in key_list:
            if 'shift' in key_list:
                return 'shift+w'
            else:
                return 'w'
        elif 'e' in key_list:
            return 'e'
        elif 'r' in key_list:
            return 'r'
        else:
            return 'release'

    def get_key_y(self):     # 横向控制
        while True:
            key_list = keyboard.get_hotkey_name().split('+')
            # self.steer_exp = self.eps_steer
            if 'd' in key_list or 'D' in key_list:
                if self.steer_exp <= self.steerRange[0]:
                    self.steer_exp = self.steerRange[0]
                else:
                    self.steer_exp -= 10
            elif 'a' in key_list or 'A' in key_list:
                if self.steer_exp >= self.steerRange[1]:
                    self.steer_exp = self.steerRange[1]
                else:
                    self.steer_exp += 10
            elif 'z' in key_list or 'Z' in key_list:
                self.steer_exp = 0
            else:
                 continue
            time.sleep(0.01)

    def set_steer(self, keyY) -> float:
        steer = self.eps_steer
        steerDelta = self.steerDelta
        steerRange = self.steerRange
        if keyY == 'a':
            if steer >= steerRange[1]:
                steer = steerRange[1]
            else:
                steer += steerDelta
        elif keyY == 'd':
            if steer <= steerRange[0]:
                steer = steerRange[0]
            else:
                steer -= steerDelta
        elif keyY == 'z':
            steer = 0
        return steer

    def keyboard_ctl(self) -> None:
        while True:
            time.sleep(0.1)
            try:
                state = self.get_state()
                print(state)
            except ValueError as e:
                print(e)
            keyX = self.get_key_x()
            try:
                func = self.state_table[state, keyX]
            except KeyError:
                continue
            func(self, self.steer_exp)
            continue
        

    def main_loop(self):
        cansend = Thread(target=self.keyboard_ctl)
        canrecv = Thread(target=self.recv)
        Thread(target=self.get_key_y).start()
        #self.startup()
        cansend.start()
        canrecv.start()

if __name__ == '__main__':
    CarController().main_loop()
